#include "stm32f10x.h" // Device header
#include "SpeedTest.h"
#include "Motor.h"
#include "math.h"
#include "Servo.h"
#include "MPU6050.h"


float PI = 3.14159265358979323846;
/*电机速度pid变量*/

float kp = 3, ki = 0, kd = 0;
float Out1, Out2, Actual1, Actual2;
float Error0, Error1, ErrorAll1;
float Error2, Error3, ErrorAll2;
int16_t Target_Right, Target_Left;

/*角度pid变量*/

float Target_Angle;
float kp_angle = 0.02, ki_angle = 0, kd_angle = 0;
float Out_Angle, Actual_Angle;
float Error0_Angle, Error1_Angle, ErrorAll_Angle;

/*小车参数，长度单位：cm，时间单位：s*/

const float W = 16.2; //轮距
const float H = 14.4; //轴距
float Vx = 20, VL, VR,Vz; 
float K = 0.8;
#define VZMAX  (Vx / (0.5 * W))

/*电机参数*/

#define PPR 13	//编码器线数
#define C (6.5f * 3.1415)	//轮胎周长
#define REDUCTION_RATIO 28	//减速比
#define T 0.1f	//测速周期
#define PLUSES PPR*REDUCTION_RATIO	//轮胎转一圈脉冲数

int16_t Speed2Count(float v)
{
	return (int16_t)v * PLUSES * T / C;
}

void PID_Left(void)
{

	Actual1 = Encoder_Speed1;
	Error1 = Error0;
	Error0 = Target_Left - Actual1;
	ErrorAll1 += Error0;
	Out1 += kp * Error0 + ki * ErrorAll1 + kd * (Error0 - Error1);
	if (Out1 > 20000)
	{
		Out1 = 20000;
	}
	if (Out1 < -20000)
	{
		Out1 = -20000;
	}
	Motor_SetPWM1((int16_t)Out1);
}

void PID_Right(void)
{

	Actual2 = Encoder_Speed2;
	Error3 = Error2;
	Error2 = Target_Right - Actual2;
	ErrorAll2 += Error2;
	Out2 += kp * Error2 + ki * ErrorAll2 + kd * (Error2 - Error3);
	if (Out2 > 20000)
	{
		Out2 = 20000;
	}
	if (Out2 < -20000)
	{
		Out2 = -20000;
	}
	Motor_SetPWM2((int16_t)Out2);
}

/*角度pid，输出vz*/
void PID_Angle(void)
{
	Actual_Angle = Angle;
	Error1_Angle = Error0_Angle;
	Error0_Angle = Target_Angle - Angle;
	ErrorAll_Angle += Error0_Angle;
	Vz = kp_angle * Error0_Angle + ki_angle * ErrorAll_Angle + kd_angle * (Error0_Angle - Error1_Angle);
	Vz = fabsf(Vz) >= VZMAX ? VZMAX : Vz;
	float R = Vx / fabsf(Vz);
	float AngleL, AngleR;
	if(Vz >= 0)
	{
		VL = Vx * (R - 0.5 * W) / R;
		VR = Vx * (R + 0.5 * W) / R;
		AngleL = atan(H/(R-0.5*W))/ PI * 180;
		AngleR = atan(H/(R+0.5*W))/ PI * 180;
		Out_Angle = 90 + (AngleL + AngleR) / 2 / K;
	}
	else
	{
		VL = Vx * (R + 0.5 * W) / R;
		VR = Vx * (R - 0.5 * W) / R;
		AngleL = (atan(H/(R+0.5*W))) / PI * 180;
		AngleR = (atan(H/(R-0.5*W))) / PI * 180;
		Out_Angle = 90 - (AngleL + AngleR) / 2 / K;
	}
	if(Out_Angle > 130) Out_Angle = 130;
	if(Out_Angle < 50) Out_Angle = 50;
	Servo_SetAngle(Out_Angle);
}

void Control(int8_t t)
{
	Target_Angle = t;
	PID_Angle();
	Target_Right = Speed2Count(VR);
	Target_Left = Speed2Count(VL);
	PID_Right();
	PID_Left();
}
